from loadData import loadPly
from gakit import *
import random

def ransac(mvs, totThresh, thresh, numP=3, gav=True):
  print "[RANSAC]"
  print "[Found {0} points in set]".format(len(mvs))
  planes = []

  if gav:
    print "[Connecting to GAViewer]"
    s = socket(AF_INET, SOCK_STREAM)
    s.connect(('localhost', 6860))
    print "[Connected to GAViewer]"
    mvsToGAV(mvs, 300)

  # continue until all planes have been found
  print "[Looking for planes...]"
  while len(planes) < numP:

    # get three random points
    sample = random.sample(mvs, 3)

    # generate a plane from these points
    plane = sample[0] ^ sample[1] ^ sample[2] ^ ni
    plane = plane.dual()

    # check how close this plane matches rest of points, create error margin
    numinplane = 0

    for p in mvs:
      if abs(float((p | plane).s.replace(" ", ""))) < thresh:
        numinplane += 1

    if numinplane > totThresh:
      print "[Found plane]"
      print plane
      if gav:
        sendToGAV(s, "pl{0} = {1}".format(len(planes), plane))
      print sample
      planes.append(plane)
      print "[Removing points belonging to found plane...]"
      mvs = [p for p in mvs if abs(float((p | plane).s.replace(" ", ""))) > thresh]
      print "[Done removing. {0} points left]".format(len(mvs))
      totThresh *= 0.8
      print "[Decreasing threshold to {0}]".format(totThresh)

    # check if error is too high, if not, add to set of planes
#    if error < threshold:
#      # remove points that fit the found plane from the dataset
#      for p in good_points:
#        data.remove(p)
#      planes.append(plane)

  return planes
